/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "track.h"

#include <limits>

namespace fusion {

size_t Track::s_track_idx_ = 1;
double Track::s_max_lidar_invisible_period_ = 0.25;   // in second
double Track::s_max_radar_invisible_period_ = 0.50;   // in second
double Track::s_max_camera_invisible_period_ = 0.75;  // in second

Track::Track() {
    fused_object_.reset(new FusedObject());
}

bool Track::Initialize(SensorObjectPtr obj, bool is_background) {
    Reset();
    int track_id = static_cast<int>(GenerateNewTrackId());
    is_background_ = is_background;
    std::shared_ptr<base::Object> fused_base_obj = fused_object_->GetBaseObject();
    std::shared_ptr<const base::Object> sensor_base_obj = obj->GetBaseObject();
    *fused_base_obj = *sensor_base_obj;
    fused_base_obj->track_id = track_id;
    UpdateWithSensorObject(obj);
    return true;
}

void Track::Reset() {
    fused_object_->GetBaseObject()->track_id = 0;
    lidar_objects_.clear();
    radar_objects_.clear();
    camera_objects_.clear();
    tracking_period_ = 0.0;
    is_background_ = false;
    is_alive_ = true;
    tracked_times_ = 0;
    toic_prob_ = 0.0;
}

SensorObjectConstPtr Track::GetSensorObject(
    const std::string& sensor_id) const {
    auto lidar_it = lidar_objects_.find(sensor_id);
    if (lidar_it != lidar_objects_.end()) {
        return lidar_it->second;
    }

    auto radar_it = radar_objects_.find(sensor_id);
    if (radar_it != radar_objects_.end()) {
        return radar_it->second;
    }

    auto camera_it = camera_objects_.find(sensor_id);
    if (camera_it != camera_objects_.end()) {
        return camera_it->second;
    }

    return nullptr;
}

SensorObjectConstPtr Track::GetLatestLidarObject() const {
    return GetLatestSensorObject(lidar_objects_);
}

SensorObjectConstPtr Track::GetLatestRadarObject() const {
    return GetLatestSensorObject(radar_objects_);
}

SensorObjectConstPtr Track::GetLatestCameraObject() const {
    return GetLatestSensorObject(camera_objects_);
}

SensorObjectConstPtr Track::GetLatestSensorObject(
    const SensorId2ObjectMap& objects) const {
    SensorObjectConstPtr obj = nullptr;
    for (auto it = objects.begin(); it != objects.end(); ++it) {
        if (obj == nullptr || obj->GetTimestamp() < it->second->GetTimestamp()) {
            obj = it->second;
        }
    }
    return obj;
}

size_t Track::GenerateNewTrackId() {
    int ret_track_id = static_cast<int>(s_track_idx_);
    if (s_track_idx_ == std::numeric_limits<unsigned int>::max()) {
        s_track_idx_ = 1;
    } else {
        s_track_idx_++;
    }
    return ret_track_id;
}

void Track::UpdateSensorObject(SensorId2ObjectMap* objects,
                               const SensorObjectPtr& obj) {
    std::string sensor_id = obj->GetSensorId();
    auto it = objects->find(sensor_id);
    if (it == objects->end()) {
        (*objects)[sensor_id] = obj;
    } else {
        it->second = obj;
    }
}

void Track::UpdateWithSensorObject(const SensorObjectPtr& obj) {
    std::string sensor_id = obj->GetSensorId();
    SensorId2ObjectMap* objects = nullptr;
    if (IsLidar(obj)) {
        objects = &lidar_objects_;
    } else if (IsRadar(obj)) {
        objects = &radar_objects_;
    } else if (IsCamera(obj)) {
        objects = &camera_objects_;
    } else {
        return;
    }
    // NOTE(@liuxinyu): UpdateSensorObject()
    // objects取地址，根据探测来更新lidar_objects_, radar_objects_, camera_objects_
    UpdateSensorObject(objects, obj);
    double time_diff = obj->GetTimestamp() - fused_object_->GetTimestamp();
    tracking_period_ += time_diff;

    UpdateSensorObjectWithMeasurement(&lidar_objects_, sensor_id,
                                      obj->GetTimestamp(),
                                      s_max_lidar_invisible_period_);
    UpdateSensorObjectWithMeasurement(&radar_objects_, sensor_id,
                                      obj->GetTimestamp(),
                                      s_max_radar_invisible_period_);
    UpdateSensorObjectWithMeasurement(&camera_objects_, sensor_id,
                                      obj->GetTimestamp(),
                                      s_max_camera_invisible_period_);

    if (is_background_) {
        return UpdateWithSensorObjectForBackground(obj);
    }

    fused_object_->GetBaseObject()->latest_tracked_time = obj->GetTimestamp();
    UpdateSupplementState(obj);
    UpdateUnfusedState(obj);
    is_alive_ = true;
}

void Track::UpdateWithoutSensorObject(const std::string& sensor_id,
                                      double measurement_timestamp) {
    UpdateSensorObjectWithoutMeasurement(&lidar_objects_, sensor_id,
                                         measurement_timestamp,
                                         s_max_lidar_invisible_period_);
    UpdateSensorObjectWithoutMeasurement(&radar_objects_, sensor_id,
                                         measurement_timestamp,
                                         s_max_radar_invisible_period_);
    UpdateSensorObjectWithoutMeasurement(&camera_objects_, sensor_id,
                                         measurement_timestamp,
                                         s_max_camera_invisible_period_);

    UpdateSupplementState();
    // @liuxinyu: 根據時間檢查，如果某個track關聯的sensor obj爲0，則應刪除了。
    is_alive_ = (!lidar_objects_.empty()) || (!radar_objects_.empty()) ||
                (!camera_objects_.empty());
}

void Track::UpdateSensorObjectWithoutMeasurement(SensorId2ObjectMap* objects,
                                                 const std::string& sensor_id,
                                                 double measurement_timestamp,
                                                 double max_invisible_period) {
    for (auto it = objects->begin(); it != objects->end();) {
        double period = measurement_timestamp - it->second->GetTimestamp();
        if (it->first == sensor_id) {
            it->second->SetInvisiblePeriod(period);
        } else if (it->second->GetInvisiblePeriod() > 0.0) {
            it->second->SetInvisiblePeriod(period);
        }

        if (period > max_invisible_period) {
            it->second = nullptr;
            it = objects->erase(it);
        } else {
            ++it;
        }
    }
}

void Track::UpdateSensorObjectWithMeasurement(SensorId2ObjectMap* objects,
                                              const std::string& sensor_id,
                                              double measurement_timestamp,
                                              double max_invisible_period) {
    // NOTE(@liuxinyu): 当不同于探测sensor_id时，需更新自身历史数据
    for (auto it = objects->begin(); it != objects->end();) {
        if (it->first != sensor_id) {
            double period = measurement_timestamp - it->second->GetTimestamp();
            if (period > max_invisible_period) {
                it->second = nullptr;
                it = objects->erase(it);
            } else {
                ++it;
            }
        } else {
            ++it;
        }
    }
}

void Track::UpdateSupplementState(const SensorObjectPtr& src_object) {
    std::shared_ptr<base::Object> dst_obj = fused_object_->GetBaseObject();
    if (src_object != nullptr) {
        std::shared_ptr<const base::Object> src_obj = src_object->GetBaseObject();
        if (IsLidar(src_object)) {
            dst_obj->lidar_supplement = src_obj->lidar_supplement;
        } else if (IsRadar(src_object)) {
            dst_obj->radar_supplement = src_obj->radar_supplement;
        } else if (IsCamera(src_object)) {
            dst_obj->camera_supplement = src_obj->camera_supplement;
        }
    }

    if (lidar_objects_.empty()) {
        dst_obj->lidar_supplement.Reset();
    }
    if (radar_objects_.empty()) {
        dst_obj->radar_supplement.Reset();
    }
    if (camera_objects_.empty()) {
        dst_obj->camera_supplement.Reset();
    }
}

void Track::UpdateUnfusedState(const SensorObjectPtr& src_object) {
    std::shared_ptr<base::Object> dst_obj = fused_object_->GetBaseObject();
    std::shared_ptr<const base::Object> src_obj = src_object->GetBaseObject();
    if (IsLidar(src_object)) {
        dst_obj->confidence = src_obj->confidence;
        dst_obj->velocity_converged = src_obj->velocity_converged;
    } else if (IsRadar(src_object)) {
        // update nothing temporarily
    } else if (IsCamera(src_object)) {
        dst_obj->confidence = src_obj->confidence;
    }
}

bool Track::IsVisible(const std::string& sensor_id) const {
    SensorObjectConstPtr sensor_obj = GetSensorObject(sensor_id);
    return (sensor_obj != nullptr && sensor_obj->GetInvisiblePeriod() < 1.0e-6);
}

bool Track::IsLidarVisible() const {
    for (auto it = lidar_objects_.begin(); it != lidar_objects_.end(); ++it) {
        if (it->second->GetInvisiblePeriod() < 1.0e-6) {
            return true;
        }
    }
    return false;
}

bool Track::IsRadarVisible() const {
    for (auto it = radar_objects_.begin(); it != radar_objects_.end(); ++it) {
        if (it->second->GetInvisiblePeriod() < 1.0e-6) {
            return true;
        }
    }
    return false;
}

bool Track::IsCameraVisible() const {
    for (auto it = camera_objects_.begin(); it != camera_objects_.end(); ++it) {
        if (it->second->GetInvisiblePeriod() < 1.0e-6) {
            return true;
        }
    }
    return false;
}

void Track::UpdateWithSensorObjectForBackground(const SensorObjectPtr& obj) {
    std::shared_ptr<base::Object> fused_base_object =
        fused_object_->GetBaseObject();
    std::shared_ptr<const base::Object> measurement_base_object =
        obj->GetBaseObject();
    int track_id = fused_base_object->track_id;
    *fused_base_object = *measurement_base_object;
    fused_base_object->track_id = track_id;
}

void Track::UpdateWithoutSensorObjectForBackground(
    const std::string& sensor_id,
    double measurement_timestamp) {}

std::string Track::DebugString() const {
    std::ostringstream oss;
    oss << "fusion_track[id: " << this->GetTrackId() << ", fused_object("
        << fused_object_->GetBaseObject()->ToString() << ")\n";

    oss << "lidar_measurments[number: " << lidar_objects_.size() << ",";
    for (auto it = lidar_objects_.begin(); it != lidar_objects_.end(); ++it) {
        oss << "(sensor_id: " << it->first
            << ", invisible_t: " << it->second->GetInvisiblePeriod()
            << ", info: " << it->second->GetBaseObject()->ToString() << ")\n";
    }
    oss << "]\n";

    oss << "radar_measurments[number: " << radar_objects_.size() << ",";
    for (auto it = radar_objects_.begin(); it != radar_objects_.end(); ++it) {
        oss << "(sensor_id: " << it->first
            << ", invisible_t: " << it->second->GetInvisiblePeriod()
            << ", info: " << it->second->GetBaseObject()->ToString() << ")\n";
    }
    oss << "]\n";

    oss << "camera_measurments[number: " << camera_objects_.size() << ",";
    for (auto it = camera_objects_.begin(); it != camera_objects_.end(); ++it) {
        oss << "(sensor_id: " << it->first
            << ", invisible_t: " << it->second->GetInvisiblePeriod()
            << ", info: " << it->second->GetBaseObject()->ToString() << ")\n";
    }
    oss << "]\n";
    return oss.str();
}

}  // namespace fusion
